/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.team1160.breakaway.drive.pid;

/**
 *
 * @author CJ
 */
public class PID {
    protected double total_error;
    protected double last_error;

    protected double last_setValue;

    protected final double KP;
    protected final double KI;
    protected final double KD;

    public PID(
            double kP,
            double kI,
            double kD){
        this.KP = kP;
        this.KI = kI;
        this.KD = kD;
    }

    public double getMotorSpeed(double setValue, double currentValue){
        if(this.last_setValue - setValue > 0.05){
            this.last_setValue = setValue;
            this.total_error = 0.0;
            this.last_error = 0.0;
        }

        double error = this.last_setValue - currentValue;
        this.total_error += error;

        double output =  (KP * error) + (KI * total_error) + (KD * (error - this.last_error));

        this.last_error = error;
        return output;

    }

}
